#include "WPILib.h"
#include "GlobalInclude.h"
//#include "CANJaguar.h"
#include "PhotoSwitch.h"
#include "Drive.h"
#include "OperatorInterface.h"
#include "PIDCalculator.h"
#include "Arm.h"

#define waitTime 0.01//try 0.05 or 0.1

class CANRobotDemo : public IterativeRobot
{
	Drive *dr;
	OperatorInterface *oi;
	PhotoSwitch *ps;
	Arm *arm;
	Compressor *comp;

public:

	CANRobotDemo(void){

		//SmartDashboard::init();
		GetWatchdog().SetExpiration(100);

	}//end of constructor
	void RobotInit()
	{
		printf("RobotInit");
		/*****moved from constructor*****/
		wpi_stackTraceEnable(true);
		dr = new Drive();
		oi = new OperatorInterface();
		ps = new PhotoSwitch(dr);
		arm = new Arm(oi);
		comp = new Compressor(10,1);
		/*****end moved*****/
		//oi->lcd->Printf(DriverStationLCD::kMain_Line6, 1, "Bus Voltage: %f",dr->leftJag->GetBusVoltage());
	}//end of RobotInit
	void AutonomousInit(void)
	{
		printf("AutonomousInit\n");
		GetWatchdog().SetEnabled(false);
		oi->updateButtons();
		arm->liftInit();
	}//end of AutonomousInit
	void AutonomousPeriodic(void)
	{
		ps->drive(oi->ds->GetDigitalIn(1));
		dr->myRobot->TankDrive(ps->leftSpeed,ps->rightSpeed);
		if((ps->allLight()) || (ps->allDark()))
		{
			//run lift and arm
		}
		//SmartDashboard::Log
		/*
		printf("onDark: %i\n", ps->onDark());
		printf("onDark1: %i\n", ps->onDark1());
		printf("onDark3: %i\n", ps->onDark3());
		printf("dleft: %i\n", ps->dLeft());
		printf("dRight: %i\n", ps->dRight());
		printf("allDark: %i\n", ps->allDark());
		*/
	}//end of AutonomousPeriodic

	void TeleopInit(void)
	{
		printf("TeleOpInit\n");
		GetWatchdog().SetEnabled(false);
		arm->liftInit();
		oi->updateButtons();
		comp->Start();
	}//end of TeleopInit
	void TeleopPeriodic(void)
	{
		//printf("my teleopPeriodic\n");
	}//end of TeleOpPeriodic
	void TeleopContinuous(void)
	{
		//printf("my teleopContinuous\n");
		//printf("killSw true\n");
		dr->myRobot->TankDrive(oi->rightStick,oi->leftStick); // drive with tank style
		arm->liftSubroutine();
		arm->armSubroutine();
		if(oi->rightStick->GetTrigger())
		{
			dr->shifter->Set(DoubleSolenoid::kForward);//high gear
		}else if(oi->leftStick->GetTrigger())
		{
			dr->shifter->Set(DoubleSolenoid::kReverse);//low gear
		}else
		{
			//do nothing
		}
		//printf("its working");
		if(oi->killSw == true)
		{
			
		}else if (oi->killSw == false){
			//comp->Stop();
		}//end of else
	}
	
	//void TeleOpContinuous(){} not currently used
	void DisabledInit(void)
	{
		printf("DisabledInit\n");
	}//end of DisabledInit
	
	void DisabledPeriodic(void)
	{
		//printf("Disabled\n");
		//pid->Disable();
		//gyro->Reset();
		//delete ps;	
		//ps = NULL;
		//dr->leftJag->Set(0.0);
		//dr->rightJag->Set(0.0);
		//comp->Stop();
	}//end of DisabledPeriodic
	
	//void DisabledContinuous(void){} not currently used

};//end of CANRobotDemo Class

START_ROBOT_CLASS(CANRobotDemo);

